#include "Scene.h"


namespace iusc 
{
    void IUSCIndoorScene::initialize(ros::NodeHandle& nh,ros::NodeHandle& pnh)
    {
        m_nh = nh;
        m_pnh = pnh;
        m_setLinkStateClient = m_nh.serviceClient<gazebo_msgs::SetLinkState>("/gazebo/set_link_state");
        m_getLinkStateClient = m_nh.serviceClient<gazebo_msgs::GetLinkState>("/gazebo/get_link_state");
        m_refereeUGVPub = m_nh.advertise<geometry_msgs::PoseArray>("/referee/ugv",1);
        sleep(5);  // 等待gazebo启动
        // m_nh.param("config_filename",m_configFilename,std::string(""));
        loadConfig();    
        // 初始化对象相关参数
        initializeObjects();
        // 得先等初始化
        m_formationServer.reset(new dynamic_reconfigure::Server<iusc_indoor_env::FormationConfig>(m_pnh));
        m_formationServer->setCallback(boost::bind(&IUSCIndoorScene::formationCfgCallback,this,_1,_2));
        m_lastPubStamp = ros::Time::now();
    }

    /**
     * @brief 更新对象并通过gazebo接口设置位置
     * 
     * @param dt 间隔时间，单位为s
     */
    void IUSCIndoorScene::update(double dt)
    {
        if (!m_cfgInitialized){  // 这个时候formation没有赋值，不能update
            return;
        }
        for (auto o:m_updateObjects)
        {
            o->update(dt);
            if (o->Type() == CustomObjectID::UGV_FORMATION)
            {  // 编队要以编队的形式特殊处理
                std::vector<geometry_msgs::PoseStamped> poses;
                auto formation = std::dynamic_pointer_cast<UGVFormation>(o);
                formation->getGroundTruth(poses);
                ros::Time stamp = ros::Time::now();
                geometry_msgs::PoseArray msg;
                msg.header.stamp = stamp;
                msg.header.frame_id = "map";
                for (auto p:poses)
                {
                    gazebo_msgs::SetLinkState objstate;
                    objstate.request.link_state.link_name = p.header.frame_id;
                    objstate.request.link_state.pose = p.pose;
                    // objstate.request.link_state.twist =
                    objstate.request.link_state.reference_frame = "world";
                    m_setLinkStateClient.call(objstate);
                    // 不知道为啥即使设置了joint也没有一块更新，这里手动设置led的pose
                    std::string ledName = p.header.frame_id + "_led";
                    objstate.request.link_state.link_name = ledName;
                    objstate.request.link_state.pose = p.pose;
                    // objstate.request.link_state.twist =
                    objstate.request.link_state.reference_frame = "world";
                    m_setLinkStateClient.call(objstate);
                    geometry_msgs::Pose pose;
                    pose = p.pose;
                    pose.position.z = 1.0;  // 这里定死
                    msg.poses.push_back(pose);
                }
                double dt = (stamp - m_lastPubStamp).toSec();
                if (dt > 2.0){
                    m_lastPubStamp = stamp;
                    m_refereeUGVPub.publish(msg);
                }
            }
            else 
            {
                geometry_msgs::PoseStamped pose;
                o->getPose(pose);
                gazebo_msgs::SetLinkState objstate;
                objstate.request.link_state.link_name = pose.header.frame_id;
                objstate.request.link_state.pose = pose.pose;
                objstate.request.link_state.reference_frame = "world";
                m_setLinkStateClient.call(objstate);
            }

        }
    }

    void IUSCIndoorScene::loadConfig()
    {
        parseUGVFormationNodes();
    }

    void IUSCIndoorScene::parseUGVFormationNodes()
    {  
        std::shared_ptr<UGVFormation> formation = std::make_shared<UGVFormation>();
        for (int i=0;i<m_formationParam.num;i++)
        {
            UGVParams p;
            p.link = "ugv" + std::to_string(i) + +"::ugv"+std::to_string(i)+ "_link";
            std::shared_ptr<UGV> ugv = std::make_shared<UGV>();
            ugv->setParam(p);
            formation->addUGV(ugv);
        }
        m_updateObjects.emplace_back(std::dynamic_pointer_cast<UpdateObject>(formation));
        m_formationObj = formation;
        ROS_INFO("=== Configure UGV Formation Finished! ===");
    }

    void IUSCIndoorScene::initializeObjects()
    {  // 初始化各类对象
        for (auto o:m_updateObjects){
            o->initialize();
            if (o->Type() == CustomObjectID::UGV_FORMATION)
            {  // 编队要先读取gazebo中所有ugv的位置，先设置完成
                std::vector<geometry_msgs::PoseStamped> poses;
                auto formation = std::dynamic_pointer_cast<UGVFormation>(o);
                std::vector<UGV::Ptr> ugvs;
                formation->getAllUGVs(ugvs);
                std::vector<formation::Point> initPs;
                for (auto ugv:ugvs){
                    std::string id = ugv->ID();
                    gazebo_msgs::GetLinkState reqstate;
                    reqstate.request.link_name = id;
                    reqstate.request.reference_frame = "world";
                    bool ret = m_getLinkStateClient.call(reqstate);
                    if (ret){
                        formation::Point pt;
                        pt.x = reqstate.response.link_state.pose.position.x;
                        pt.y = reqstate.response.link_state.pose.position.y;
                        pt.z = reqstate.response.link_state.pose.position.z;
                        initPs.emplace_back(pt);
                        std::cout<<"id:"<<id<<" pose:"<<pt.x<<","<<pt.y<<std::endl;
                    }
                }
                formation->setUGVInitPoses(initPs);  // 设置位置
            }
            else 
            {
                geometry_msgs::PoseStamped pose;
                o->getPose(pose);
                gazebo_msgs::SetLinkState objstate;
                objstate.request.link_state.link_name = pose.header.frame_id;
                objstate.request.link_state.pose = pose.pose;
                // objstate.request.link_state.twist =
                objstate.request.link_state.reference_frame = "world";
                m_setLinkStateClient.call(objstate);
            }
        }

        ROS_INFO("=== Initialize Scene Finished! ===");
    }

    void IUSCIndoorScene::formationCfgCallback(iusc_indoor_env::FormationConfig& cfg, uint32_t level)
    {
        if (!m_cfgInitialized){
            // 先对formation赋值
            m_cfgInitialized = true;
            m_formationParam.distance = cfg.distance;
            m_formationParam.formationType = cfg.formation_type;
            m_formationParam.moveType = cfg.traj_type;  // 移动类型
            m_formationParam.vmax = cfg.velocity;
            m_formationParam.x = cfg.x;
            m_formationParam.y = cfg.y;
            m_formationParam.vx = 0;
            m_formationParam.vy = 0;
            m_formationObj->setParam(m_formationParam);  // 设置参数
            // 设置无人车是否可降落
            std::vector<UGV::Ptr> ugvs;
            m_formationObj->getAllUGVs(ugvs);
            for (int i=0; i<ugvs.size();i++){
                auto ugv = ugvs[i];
                UGVParams p;
                ugv->getParam(p);
                switch (i)
                {
                    case 0 : {p.landed = cfg.ugv0_landed;break;}
                    case 1 : {p.landed = cfg.ugv1_landed;break;}
                    case 2 : {p.landed = cfg.ugv2_landed;break;}
                    case 3 : {p.landed = cfg.ugv3_landed;break;}
                    case 4 : {p.landed = cfg.ugv4_landed;break;}
                    case 5 : {p.landed = cfg.ugv5_landed;break;}
                    case 6 : {p.landed = cfg.ugv6_landed;break;}
                    case 7 : {p.landed = cfg.ugv7_landed;break;}
                    case 8 : {p.landed = cfg.ugv8_landed;break;}
                    case 9 : {p.landed = cfg.ugv9_landed;break;}
                
                default:
                    break;
                }
                ugv->setParam(p);
            }
            lastCfg = cfg;
            return ;
        }
        if (level == 1){
            if (cfg.stop){
                m_formationObj->stop();
            }
            if (!cfg.stop && lastCfg.stop){
                m_formationObj->Continue();
            }
            if (cfg.start){
                m_formationObj->start();
            }
        }
        else {
            if (cfg.reset){
                m_formationParam.distance = cfg.distance;
                m_formationParam.formationType = cfg.formation_type;
                m_formationParam.moveType = cfg.traj_type;  // 移动类型
                m_formationParam.vmax = cfg.velocity;
                m_formationParam.x = cfg.x;
                m_formationParam.y = cfg.y;
                m_formationParam.vx = 0;
                m_formationParam.vy = 0;
                m_formationObj->setParam(m_formationParam);  // 设置参数
                m_formationObj->reset(m_formationParam);
                // 设置无人车是否可降落
                std::vector<UGV::Ptr> ugvs;
                m_formationObj->getAllUGVs(ugvs);
                for (int i=0; i<ugvs.size();i++){
                    auto ugv = ugvs[i];
                    UGVParams p;
                    ugv->getParam(p);
                    switch (i)
                    {
                        case 0 : {p.landed = cfg.ugv0_landed;break;}
                        case 1 : {p.landed = cfg.ugv1_landed;break;}
                        case 2 : {p.landed = cfg.ugv2_landed;break;}
                        case 3 : {p.landed = cfg.ugv3_landed;break;}
                        case 4 : {p.landed = cfg.ugv4_landed;break;}
                        case 5 : {p.landed = cfg.ugv5_landed;break;}
                        case 6 : {p.landed = cfg.ugv6_landed;break;}
                        case 7 : {p.landed = cfg.ugv7_landed;break;}
                        case 8 : {p.landed = cfg.ugv8_landed;break;}
                        case 9 : {p.landed = cfg.ugv9_landed;break;}
                        
                    
                    default:
                        break;
                    }
                    ugv->setParam(p);
                }
            }
        }
        lastCfg = cfg;
    }

}